#include "WPILib.h"
#include "Compressor.h"
#include "DriverStationLCD.h"
#include "Encoder.h"
#include <cmath>
#include "BambooDrive.h"
#include "BambooFan.h"
#include "BambooKicker.h"
#include "vxWorks.h" 
#include "DashboardDataSender.h"
#include "DriverStationLCD.h"
#include "Target.h"
#include "Relay.h"

#define MINIMUM_SCORE 0.005

const int Fire = 5;
const int GyroReset = 7;
const double KICKERDISTANCE = 3;
const double KICKWAITTIME = 2;

double outputValue;

class SamplePIDOutput : public PIDOutput {
public:
	SamplePIDOutput(BambooDrive *bamboobase) {
		m_bamboobase = bamboobase;
	}

	void PIDWrite(float output) {
		m_bamboobase->ArcadeDrive(output, 0.0);
		outputValue = -output;
	}
private:
	BambooDrive *m_bamboobase;
};
/**
 * This is a demo program showing the use of the RobotBase class.
 * The SimpleRobot class is the base of a robot application that will automatically call your
 * Autonomous and OperatorControl methods at the right time as controlled by the switches on
 * the driver station or the field controls.
 */ 
class BambooBot : public SimpleRobot
{
	Joystick *Stick;
	BambooDrive *bambooDrive; // robot drive system
	Gyro *gyro;
	PIDOutput *pidOutput;
	DashboardDataSender *dds;
	DriverStationLCD *dslcd;
	DriverStation *ds;
	BambooFan *fan;
	Accelerometer *Xaccel;
	Accelerometer *Yaccel;
	Accelerometer *Zaccel;
	AnalogChannel *thermocouple;
	Relay *compressor;
	DigitalInput *pressureSwitch;
	DigitalInput *kickerswitch;
	BambooKicker *kicker;
	Solenoid *solenoid1;
	Solenoid *solenoid2;
public:
	BambooBot(void)
	{
		dslcd = DriverStationLCD::GetInstance();
		ds = DriverStation::GetInstance();
		Stick = new Joystick(1);
		bambooDrive = new BambooDrive(10, 8, 7, 9, dslcd);	// these must be initialized in the same order
		gyro = new Gyro(1);
		pidOutput = new SamplePIDOutput(bambooDrive);
		dds = new DashboardDataSender();
		fan = new BambooFan(4);
		Xaccel = new Accelerometer(6);
		Yaccel = new Accelerometer(7);
		Zaccel = new Accelerometer(8);
		thermocouple = new AnalogChannel(5);
		kickerswitch = new DigitalInput(4);
		kicker = new BambooKicker(6,1,2,8, Stick, kickerswitch, 4);
		compressor = new Relay(2,Relay::kReverseOnly);
		pressureSwitch = new DigitalInput(3); 
		solenoid1 = new Solenoid(6);
		solenoid2 = new Solenoid(7);
//		encoder = new Encoder(1,2, false);
//		encoder->Reset();
//		encoder->Start();
//		solenoid = new Solenoid(8);
//		winch = new Victor(6);
		GetWatchdog().SetExpiration(100);
	}

	void BambooBot::StartCompetition()
	{
		printf("Hello, this is StartCompetition.\n");
		while (1)
		{
			while (IsDisabled())
				Wait(.01);
			if (IsAutonomous())
			{
				Autonomous();
				while (IsAutonomous() && !IsDisabled()) {
					Wait(.01);
					GetWatchdog().Feed();
				}
			}
			else
			{
				OperatorControl();
				while (IsOperatorControl() && !IsDisabled()) {
					Wait(.01);
					GetWatchdog().Feed();
				}
			}
		}
	}
	/**
	 * Drive left & right motors for 2 seconds then stop
	 */
	void Autonomous(void)
	{
		/*
		static int autoCounter = 0;
		double area;
		double targetAreaOne; //closest
		double targetAreaTwo; //middle
		double targetAreaThree; //farthest
		double targetLimit;
		
		AxisCamera &camera = AxisCamera::GetInstance();
		printf("Setting camera parameters\n");
		camera.WriteResolution(AxisCamera::kResolution_320x240);
		camera.WriteCompression(20);
		camera.WriteBrightness(0);
		
		Timer timer;
		timer.Start();
		
		while(IsAutonomous()) {
			GetWatchdog().Feed();	

			
			if (camera.IsFreshImage()) {
				timer.Reset();
				// get the gyro heading that goes with this image
			
				// get the camera image
				HSLImage *image = camera.GetImage();

				// find FRC targets in the image
				vector<Target> targets = Target::FindCircularTargets(image);
				delete image;
				if (targets.size() == 0 || targets[0].m_score < MINIMUM_SCORE)
				{
					// no targets found. Make sure the first one in the list is 0,0
					// since the dashboard program annotates the first target in green
					// and the others in magenta. With no qualified targets, they'll all
					// be magenta.
					Target nullTarget;
					nullTarget.m_majorRadius = 0.0;
					nullTarget.m_minorRadius = 0.0;
					nullTarget.m_score = 0.0;
					if (targets.size() == 0)
						targets.push_back(nullTarget);
					else
						targets.insert(targets.begin(), nullTarget);
					dds->sendVisionData(0.0, gyro->GetAngle(), 0.0, 0.0, targets);
				}
				else
					area = targets[0].m_majorRadius*targets[0].m_minorRadius;
				//offensive zone
				if (targetAreaOne - targetLimit <= area <= targetAreaOne + targetLimit)
					continue;
				//middle zone
				else if (targetAreaTwo - targetLimit <= area <= targetAreaTwo + targetLimit)
					continue;
				//defensive zone
				else if (targetAreaThree - targetLimit <= area <= targetAreaThree + targetLimit)
					continue;
				else
					continue;
				if (autoCounter % 10000 == 0)
					printf("\nTarget Area: %f ", area);
				autoCounter ++;
			}
		}*/
		bool kick = false;
		bool prevKick = false;
		int kickcounter = 1;
		GetWatchdog().SetExpiration(100);
			while(IsAutonomous()) {
				GetWatchdog().Feed();
				double curTime = GetTime();
				static double startTime = GetTime();
				double deltaTime = curTime - startTime;
				
				if (!pressureSwitch->Get())
					compressor->Set(Relay::kOn);
				if (pressureSwitch->Get())
					compressor->Set(Relay::kOff);
				
				if (kickcounter <= 3) {
					if (kick) 
						kicker->Kick(1);
					else
						kicker->Kick(0);
					if (deltaTime >= kick*KICKWAITTIME) {
						if (prevKick == 1) {
							kick = false;
							kickcounter++;
							startTime = GetTime();
						}
						if (deltaTime < KICKERDISTANCE)
							bambooDrive->SetAllWheelMotorSpeeds(.4,.4,.4,.4);
						else {
							bambooDrive->SetAllWheelMotorSpeeds(0,0,0,0);
							kick = true;
							startTime = GetTime();
						}
					} else {
						bambooDrive->SetAllWheelMotorSpeeds(0,0,0,0);
					}
					prevKick = kick;
				} else {
					kicker->Kick(0);
					bambooDrive->SetAllWheelMotorSpeeds(0,0,0,0);
				}
			}
	}

	/**
	 * Runs the motors with arcade steering. 
	 */
	void OperatorControl(void)
	{
		unsigned int counter = 0;
		bool firing = false;
		int driveState;
		printf("Build 14: We are initializing PIDController\n");
		PIDController turnController( .6, // P
				0.2, // I
				0.1, // D
				gyro, // source
				pidOutput, // output
				0.005); // period
		turnController.SetInputRange(-360.0, 360.0);
		turnController.SetOutputRange(-.8, .8);
		turnController.SetTolerance(1.0 / 90.0 * 100);
		turnController.Disable();

		// Create and set up a camera instance. first wait for the camera to start
		// if the robot was just powered on. This gives the camera time to boot.
		printf("Getting camera instance\n");
		AxisCamera &camera = AxisCamera::GetInstance();
		printf("Setting camera parameters\n");
		camera.WriteResolution(AxisCamera::kResolution_320x240);
		camera.WriteCompression(20);
		camera.WriteBrightness(0);
		
		// set sensitivity for the 2010 kit gyro
		gyro->SetSensitivity(0.007);
		
		// set watchdog
		GetWatchdog().SetExpiration(100);

		// keep track of the previous joystick trigger value                
		Timer timer;
		timer.Start();

		// loop getting images from the camera and finding targets
		printf("\nStarting operator control loop");
		while (IsOperatorControl()) {
			
			GetWatchdog().Feed();

			if (Stick->GetRawButton(8)) {
				solenoid1->Set(true);
				solenoid2->Set(false);
			} else if (Stick->GetRawButton(9)) {
				solenoid1->Set(false);
				solenoid2->Set(true);
			}
			/*if(encoder->Get() < 800)
				winch->Set(1);
			else
				winch->Set(0);*/
			if (!pressureSwitch->Get())
				compressor->Set(Relay::kOn);
			if (pressureSwitch->Get())
				compressor->Set(Relay::kOff);
		    if(Stick->GetRawButton(GyroReset))
		    	gyro->Reset();
			// if trigger is pulled, the robot will run with standard arcade drive
			// otherwise the robot will home towards the target.
				if (Stick->GetRawButton(Fire)) {
					if (!firing) {
						turnController.Enable();
						firing = true;
					}
					// if there's a fresh and we're at the previous target heading then
					// get a camera image and process it
					if (camera.IsFreshImage()) {
						timer.Reset();
						// get the gyro heading that goes with this image
						double gyroAngle = gyro->PIDGet();
						
						// get the camera image
						HSLImage *image = camera.GetImage();

						// find FRC targets in the image
						vector<Target> targets = Target::FindCircularTargets(image);
						delete image;
						if (targets.size() == 0 || targets[0].m_score < MINIMUM_SCORE)
						{
							// no targets found. Make sure the first one in the list is 0,0
							// since the dashboard program annotates the first target in green
							// and the others in magenta. With no qualified targets, they'll all
							// be magenta.
							Target nullTarget;
							nullTarget.m_majorRadius = 0.0;
							nullTarget.m_minorRadius = 0.0;
							nullTarget.m_score = 0.0;
							if (targets.size() == 0)
								targets.push_back(nullTarget);
							else
								targets.insert(targets.begin(), nullTarget);
							dds->sendVisionData(0.0, gyro->GetAngle(), 0.0, 0.0, targets);
							if (counter % 100 == 0)
								if (targets.size() == 0)
									printf("\nNo target found");
								else
									printf("\nNo valid targets found, best score: %f ", targets[0].m_score);
						} else {
							// We have some targets.
							// set the new PID heading setpoint to the first target in the list
							double horizontalAngle = targets[0].GetHorizontalAngle();
							double setPoint = gyroAngle + horizontalAngle;
							
						turnController.SetSetpoint(setPoint);
						
							// send dashbaord data for target tracking
							dds->sendVisionData(0.0, gyro->GetAngle(), 0.0, targets[0].m_xPos / targets[0].m_xMax, targets);
							if (counter % 100 == 0) {
								printf("\nTarget found %f ", targets[0].m_score);

								targets[0].Print();
							}
						}	
						if (counter % 100 == 0)
							printf("\nTime: %f", 1.0 / timer.Get());
						dslcd->Printf(DriverStationLCD::kUser_Line4, 1, "        %.2f   ", targets[0].GetHorizontalAngle());
					}
			} else {
				// if the trigger is pressed, then do Arcade driving with joystick 1
				if (firing) {
					turnController.Disable();
					firing = false;
				}
				if (Stick->GetTrigger())
					driveState = bambooDrive->Teleop(Stick, gyro->GetAngle());
				else {
					bambooDrive->SetAllWheelMotorSpeeds(0,0,0,0);
					driveState = 0;
				}
			}


			float kickerState = kicker->Teleop();
				
			fan->Teleop();
	
			dslcd->Printf(DriverStationLCD::kUser_Line1, 1, "Drive : %d", driveState);
			dslcd->Printf(DriverStationLCD::kUser_Line3, 1, "Angle : ");
//			dslcd->Printf(DriverStationLCD::kUser_Line2, 1, "XAccel : %.2f YAccel : %.2f ZAccel : %.2f", Xaccel->GetAcceleration(), Yaccel->GetAcceleration(), Zaccel->GetAcceleration());
//			printf("\nEncoder Val : %d", encoder->Get());
//			dslcd->Printf(DriverStationLCD::kUser_Line3, 1, "Pressure Switch : %d", pressureSwitch->Get());
			if(kickerState == 0)
				dslcd->Printf(DriverStationLCD::kUser_Line2, 1, "Kicker : Fired   ");
			else if (kickerState == 1)
				dslcd->Printf(DriverStationLCD::kUser_Line2, 1, "Kicker : Latching");
			else if (kickerState == 2)
				dslcd->Printf(DriverStationLCD::kUser_Line2, 1, "Kicker : Wound   ");
			else if (kickerState == 3)
				dslcd->Printf(DriverStationLCD::kUser_Line2, 1, "Kicker : Primed  ");
			dslcd->Printf(DriverStationLCD::kUser_Line3, 1, "Gyro : %.2f            ", gyro->GetAngle());
			dslcd->UpdateLCD();
			
			counter++;
			
			// send the dashbaord data associated with the I/O ports
 			dds->sendIOPortData();
		}
	}
	
	void SelfTest(void) {
		static int counter = 0;
		GetWatchdog().SetExpiration(100);
		while(IsOperatorControl()) {
			GetWatchdog().Feed();
/*		    if (compressor->GetPressureSwitchValue() < 60)
		    {
		    	compressor->Start();
		    }
		    if (compressor->GetPressureSwitchValue() >= 120)
		    {
		    	compressor->Stop();
		    }*/
		    if (counter % 1000 == 0) {
//			printf("\nEncoder Val : %d", encoder->Get());
//			printf("\nSwitch Val : %d", kickerswitch->GetTriggerState());
		    }
//			solenoid->Set(true);
//			winch->Set(1);
			counter++;
		}
	}
};

START_ROBOT_CLASS(BambooBot);
